package com.hitqz.robot.driver.util;

import cn.hutool.core.util.HexUtil;
import com.google.common.util.concurrent.RateLimiter;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.communication.TcpServer;
import com.hitqz.robot.driver.dto.GfRobotStatus;
import com.hitqz.robot.driver.netty.handler.GfMessageHandler;
import com.hitqz.robot.driver.netty.handler.IpBlacklistHandler;
import com.hitqz.robot.driver.netty.protocol.decoder.GfDecoder;
import com.hitqz.robot.driver.netty.protocol.encoder.GfEncoder;
import com.hitqz.robot.driver.netty.protocol.message.GfMessage;
import io.netty.buffer.ByteBuf;
import io.netty.channel.Channel;
import io.netty.channel.ChannelHandlerContext;
import io.netty.channel.ChannelInitializer;
import io.netty.handler.codec.LengthFieldBasedFrameDecoder;
import lombok.extern.slf4j.Slf4j;

import java.util.Collections;
import java.util.HashSet;
import java.util.Optional;
import java.util.Set;
import java.util.concurrent.*;

@Slf4j
public class GfToolkit {
    //黑名单防止编码不一致
    public static final Set<String> blacklist = Collections.synchronizedSet(new HashSet<>());
    //机器人编码
    public static String serialNumber = "";
    //默认端口
    private final static int defaultPort = 1883;
    //本地server
    public static TcpServer server;
    //获取消息结果
    public static final ConcurrentHashMap<String, CompletableFuture<Boolean>> requestMap = new ConcurrentHashMap<>();
    //消息通道
    public static ChannelHandlerContext ctx;
    //远程继电器通道
    public static ChannelHandlerContext relayCtx;
    //心跳令牌桶限流
    private static final RateLimiter heartbeatsLimiter = RateLimiter.create(2);

    public static boolean init(Integer port) {
        int bindPort = port == null ? defaultPort : port;
        if (server == null) {
            server = new TcpServer("gf", bindPort, new ChannelInitializer<Channel>() {
                @Override
                protected void initChannel(Channel channel) throws Exception {
                    channel.pipeline().addLast("ipHandler", new IpBlacklistHandler(blacklist));
                    channel.pipeline().addLast("frameDecoder", new LengthFieldBasedFrameDecoder(1024, 2, 1, 1, 0));
                    channel.pipeline().addLast("decoder", new GfDecoder());
                    channel.pipeline().addLast("encoder", new GfEncoder());
                    channel.pipeline().addLast("handler", new GfMessageHandler());
                }
            });
            NetworkServiceThreadPool.creatNetworkServerOrClient(() -> {
                try {
                    server.bind();
                } catch (InterruptedException e) {
                    log.error("[gf] server bind error", e);
                }
            });
        }
        return true;
    }


    /*****************************************************************************************************/


    /**
     * 设置机器人电机转速
     *
     * @param qdMotorSpeed 驱动电机转速值 r/min
     * @param gsMotorSpeed 滚刷电机转速值 r/min
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setMotorSpeed(int qdMotorSpeed, int gsMotorSpeed) {
        byte mainCmd = 0x79;
        byte subCmd = 0x01;
        ByteBuf buffer = ctx.alloc().buffer(14);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeIntLE(qdMotorSpeed);
            buffer.writeIntLE(gsMotorSpeed);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            //boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set driver speed:{} R/min,brush speed:{} R/min",qdMotorSpeed,gsMotorSpeed);
            //return result;
            return true;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 设置机器工作方向及次数
     *
     * @param mode    机器人工作方向 0x01：设置正向 0x02：设置反向
     * @param workNum 工作次数
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setWorkMode(byte mode, byte workNum) {
        log.info("设置参数的{}--{}", mode, workNum);
        byte mainCmd = 0x79;
        byte subCmd = 0x02;
        ByteBuf buffer = ctx.alloc().buffer(8);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeByte(mode);
            buffer.writeByte(workNum);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set work mode:{},workNum:{},{}", mode == (byte) 1 ? "正向" : "反向",workNum, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 设置机器工作情况
     *
     * @param mode         机器人工作方向 0x01：设置正向 0x02：设置反向
     * @param qdMotorSpeed 驱动电机转速值
     * @param gsMotorSpeed 滚刷电机转速值
     * @param workNum      工作次数
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setWork(byte mode, int qdMotorSpeed, int gsMotorSpeed, byte workNum) {
        log.info("设置参数的{}--{}--{}--{}", mode, qdMotorSpeed, gsMotorSpeed, workNum);
        byte mainCmd = 0x79;
        byte subCmd = 0x03;
        ByteBuf buffer = ctx.alloc().buffer(16);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeByte(mode);
            buffer.writeIntLE(qdMotorSpeed);
            buffer.writeIntLE(gsMotorSpeed);
            buffer.writeByte(workNum);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set work:{},driver speed:{} R/min,brush speed:{} R/min,{}", mode == (byte) 1 ? "正向" : "反向",qdMotorSpeed,gsMotorSpeed , result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 设置机器人心跳间隔
     * @param interval 心跳间隔 s
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setHeartbeatInterval(int interval){
        byte mainCmd = 0x79;
        byte subCmd = 0x04;
        ByteBuf buffer = ctx.alloc().buffer(10);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeIntLE(interval);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set heartbeat interval:{}s,{}", interval , result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }


    /**
     * 设置光电避障使能及时间
     * @param enable 机器人光电避障功能 0x01：设置开启 0x02：设置关闭
     * @param interval 设置光电避障判断时间 ms
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setPhotoelectricObstacleAvoidance(byte enable,int interval){
        byte mainCmd = 0x79;
        byte subCmd = 0x05;
        ByteBuf buffer = ctx.alloc().buffer(11);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeByte(enable);
            buffer.writeIntLE(interval);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set set photoelectric obstacle avoidance enable:{},interval:{}s,{}",enable == (byte)0x01, interval , result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }



    /**
     * 设置IO口使能
     * @param number 0x01：相机与雨刷使能  0x02：行走电机、编码器、漫反射使能 0x03：滚刷电机使能
     * 0x04：工控机使能 0x05：推杆PE0使能 0x06：推杆PE1使能 其中：PE1置高，PE0置低，推杆深出 PE1置低，PE0置高，推杆收回
     * @param enable IO开关设置 0x01：设置开启 0x02：设置关闭
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setIoEnable(byte number,byte enable){
        byte mainCmd = 0x79;
        byte subCmd = 0x06;
        ByteBuf buffer = ctx.alloc().buffer(10);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeByte(number);
            buffer.writeByte(enable);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set set IO:{} enable:{},{}",number,enable, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 光伏功能使能
     * @param function 功能选择 0x01：推杆深出使能 0x02：推杆缩回使能 0x03：雨刷清洗 0x04：编码器清零
     *                 0x05：编码器设置顺时针方向为递增方向 0x06：编码器设置逆时针方向为递增方向
     *                 0x07：设置光伏板左侧为机器人起点 0x08：设置光伏板右侧为机器人起点
     *                 0x10：设置停电保护 0x11：设置堵转保护
     * @param enable 功能开关设置 0x01：设置开启 0x02：设置关闭
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setFunctionEnable(byte function,byte enable){
        byte mainCmd = 0x79;
        byte subCmd = 0x07;
        ByteBuf buffer = ctx.alloc().buffer(10);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeByte(function);
            buffer.writeByte(enable);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set function:{} enable:{},{}",function,enable, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }


    /**
     * 设置机器行走至预设光伏板点位
     * @param sn 设置光伏板点位
     * @param qdMotorSpeed 驱动轮电机转速（R/min）
     * @param gsMotorSpeed 滚刷电机转速（R/min）
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean goPanel(short sn,int qdMotorSpeed,int gsMotorSpeed){
        byte mainCmd = 0x79;
        byte subCmd = 0x08;
        ByteBuf buffer = ctx.alloc().buffer(16);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeShortLE(sn);
            buffer.writeIntLE(qdMotorSpeed);
            buffer.writeIntLE(gsMotorSpeed);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]go panel:{},qd motor speed:{}R/min,gs motor speed:{}R/min,{}",sn,qdMotorSpeed,gsMotorSpeed, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 设置机器行走至预设编码值
     * @param mileage 预设编码值
     * @param qdMotorSpeed 驱动轮电机转速（R/min）
     * @param gsMotorSpeed 滚刷电机转速（R/min）
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean goMileage(int mileage,int qdMotorSpeed,int gsMotorSpeed){
        byte mainCmd = 0x79;
        byte subCmd = 0x09;
        ByteBuf buffer = ctx.alloc().buffer(16);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            buffer.writeIntLE(mileage);
            buffer.writeIntLE(qdMotorSpeed);
            buffer.writeIntLE(gsMotorSpeed);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]go mileage:{},qd motor speed:{}R/min,gs motor speed:{}R/min,{}",mileage,qdMotorSpeed,gsMotorSpeed, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    public static boolean getFirmwareSupportUpdateInfo(short crc,int length){
        byte cmd =(byte) 0xf2;
        ByteBuf buffer = ctx.alloc().buffer(1);
        try {
            buffer.writeByte(cmd);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, false);
            log.info("[gf]get firmware support update info,{}", result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    public static boolean setFirmwareInfo(short crc,int length){
        byte cmd =(byte) 0xf4;
        ByteBuf buffer = ctx.alloc().buffer(7);
        try {
            buffer.writeByte(cmd);
            buffer.writeShortLE(crc);
            buffer.writeIntLE(length);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, false);
            log.info("[gf]set update firmware info crc:{},length:{},{}",crc,length, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    public static boolean sendFirmwareBlockData(short crc,int length){
        byte cmd =(byte) 0xf5;
        ByteBuf buffer = ctx.alloc().buffer(7);
        try {
            buffer.writeByte(cmd);
            buffer.writeShortLE(crc);
            buffer.writeIntLE(length);
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set update firmware info crc:{},info:{},{}",crc,length, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }


    /**
     * 获取机器工作状态
     *
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static GfRobotStatus getRobotStatus() {
        byte mainCmd = 0x79;
        byte subCmd = 0x11;
        ByteBuf buffer = ctx.alloc().buffer(6);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
            return Optional.ofNullable((GfRobotStatus) GfCacheUtil.get(GfCacheUtil.KEY_ROBOT_STATUS)).orElse(GfRobotStatus.builder().build());
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 控制继电器
     * @param sn  1-继电器1  2-继电器2
     * @param cmd  0-关 1-开
     * @return  true-发送成功 false-发送失败
     */
    public static boolean setRelayStatus(byte sn,byte cmd) {
        ByteBuf buffer = relayCtx.alloc().buffer(4);
        try {
            buffer.writeByte(0x80);
            buffer.writeByte(0x01);
            buffer.writeByte(sn);
            buffer.writeByte(cmd);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            relayCtx.writeAndFlush(gfMessage);
            boolean result = waitForResponse(gfMessage, true);
            log.info("[gf]set relay status {},{}",cmd, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏  倒转
            buffer.release();
        }
    }


    public static void sendHeartBeat() {
        if (!heartbeatsLimiter.tryAcquire()){
            return;
        }
        byte mainCmd = 0x79;
        byte subCmd = 0x12;
        ByteBuf buffer = ctx.alloc().buffer(6);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeBytes(HexUtil.decodeHex(GfToolkit.serialNumber));
            // 获取 byteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            GfMessage gfMessage = new GfMessage(bytes);
            ctx.writeAndFlush(gfMessage);
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }


/*****************************************************************************************************/


    /**
     * 发送数据位只有一个主命令字节的命令
     *
     * @param cmd 指令字节
     * @return 返回是否成功发送指令
     */
    private static boolean sendSimpleCmd(byte cmd) {
        byte[] date = new byte[]{cmd};
        GfMessage gfMessage = new GfMessage(date);
        ctx.writeAndFlush(gfMessage);
        return waitForResponse(gfMessage, false);
    }

    /**
     * 发送数据位只有主命令和子命令字节的命令
     *
     * @param mainCmd 主命令字节
     * @param subCmd  子命令字节
     * @return 返回是否成功发送指令
     */
    private static boolean sendSimpleCmd(byte mainCmd, byte subCmd) {
        byte[] date = new byte[]{mainCmd, subCmd};
        GfMessage gfMessage = new GfMessage(date);
        ctx.writeAndFlush(gfMessage);
        return waitForResponse(gfMessage, false);
    }


    /**
     * 将字节数组转为十六进制字符串
     *
     * @param bytes 字节数组
     * @return 十六进制字符串
     */
    public static String bytesToHexString(byte[] bytes) {
        StringBuilder hexString = new StringBuilder();
        for (int i = 0; i < bytes.length; i++) { // 将字节转为无符号的十六进制字符串
            String hex = String.format("%02X", bytes[i] & 0xFF);
            hexString.append(hex);
            if (i < bytes.length - 1) {
                hexString.append(" ");
            }
        }
        return hexString.toString();
    }

    public static boolean waitForResponse(GfMessage gfMessage, boolean hasSubCmd) {
        CompletableFuture<Boolean> future = new CompletableFuture<>();
        byte[] data = gfMessage.getData();
        String hexKey;
        if (hasSubCmd) {
            //有子命令的
            hexKey = String.format("%02X%02X", data[0], data[1]);
        } else {
            //没有子命令的
            hexKey = String.format("%02X", data[0]);
        }
        requestMap.put(hexKey, future);
        try {
            return future.get(2, TimeUnit.SECONDS);
        } catch (InterruptedException | ExecutionException e) {
            throw new RuntimeException(e);
        } catch (TimeoutException e) {
            log.warn("[gf]cmd:{} send timeout", gfMessage.getCmdHexStr());
        }
        return false;
    }

    public static void release(GfMessage gfMessage, boolean hasSubCmd, boolean result) {
        byte[] data = gfMessage.getData();
        String hexKey;
        if (hasSubCmd) {
            //有子命令的
            hexKey = String.format("%02X%02X", data[0], data[1]);
        } else {
            //没有子命令的
            hexKey = String.format("%02X", data[0]);
        }
        CompletableFuture<Boolean> future = requestMap.remove(hexKey);
        if (future != null) {
            future.complete(result);
            log.info("[gf]cmd:{},release", gfMessage.getCmdHexStr());
        }
    }
}
